(defvar names* nil)
(defvar vnames* nil)
(defvar learned-skills*)

(setq names* nil)
(setq vnames* nil)
(setq learned-skills* nil)

;***********************************************************************
;                       Code for Skill Learning
;***********************************************************************

; SSTORE-AND-POP pops the goal from the stack and creates a new skill
; clause that has the achieved goal as its head and the intention as its
; single subskill. It returns the new skill instance, as in ((on 5) A B).
; For now, the new clause has no start condition.

(defun sstore-and-pop (goal goal-stack)
  (let* ((intention (goal-intention goal))
	 (ihead (sinstance-goal intention))
	 (precursor (goal-precursor goal))
         (stack-entry (goal-path goal))
         (objective (goal-objective goal))
         (supergoal (cadr goal-stack))
	 (new-instance nil))
    (cond ((not (null stack-entry))
           (setf (goal-path goal) nil)))
    (cond ((eq (car objective) (car ihead))
	   (setq new-instance intention))
	  ((null precursor)
	   (setq new-instance (create-skill-one objective ihead
;					(list (sinstance-start intention)))))
						(sinstance-start intention))))
	  (t (setq new-instance
		   (create-skill-skill objective
				       (sinstance-goal precursor) ihead
				       (sinstance-start precursor)))))
   (cond ((not (null supergoal))
	  (store-with-supergoal (goal-chaintype supergoal) new-instance
				(goal-objective goal) supergoal)
          (setf (goal-sfailed supergoal) nil)
          (popd goal-stack))
	 (t
	  (clear-goal-attributes goal)))))

(defun clear-goal-attributes (goal)
  (setf (goal-chaintype goal) nil)
  (setf (goal-skills goal) nil)
  (setf (goal-subgoals goal) nil)
  (setf (goal-achieved goal) nil)
  (setf (goal-intention goal) nil)
  (setf (goal-precursor goal) nil)
  (setf (goal-sfailed goal) nil)
  (setf (goal-cfailed goal) nil)
  (setf (goal-path goal) nil)
  (setf (goal-history goal) nil))

; CREATE-SKILL-SKILL inputs a desired concept instance and a pair of skill
; instances that have been chained to achieve it. The function creates an
; hierarchical skill with the same name as the objective and the two skills
; as its subskills. The start conditions of the new skill are the same as
; those for the clause of the first subskill; since this has the same name
; as the start condition for the clause of the second subskill, it should
; be guaranteed to achieve the objective. The function involves no analytical
; collection of conditions.
; The current version includes the start condition of the single subskill,
; but later we should raise conditions from lower in the hierarchy to
; constrain search and support chaining off the learned skill clause.

(defun create-skill-skill (objective ihead1 ihead2 istart)
  (let* ((name (car objective))
	 ; Nan's change below for BINDINGS
         (bindings (generate-bindings (append (list objective ihead1 ihead2)
					      istart)
				      nil))
         (vargs (subst-bindings bindings (cdr objective)))
         (percepts (get-argument-types (mapcar #'car bindings)))
;        (start (start-instance ihead1))
         (skills (list ihead1 ihead2)))
    (cache-skill name vargs percepts istart nil skills bindings 1)))

; CREATE-SKILL-ONE does the same as CREATE-SKILL-SKILL, except it handles
; situations in which the new skill has only one subskill. 

(defun create-skill-one (objective ihead istart)
  (let* ((name (car objective))
	 ; Nan's change below for BINDINGS
	 (bindings (generate-bindings (append (list objective ihead) istart)
				      nil))
	 (vargs (subst-bindings bindings (cdr objective)))
	 (percepts (get-argument-types (mapcar #'car bindings)))
;	 (start (start-instance ihead))
	 (skills (list ihead)))
    (cache-skill name vargs percepts istart nil skills bindings 1)))

#|
; START-INSTANCE inputs a skill instance of the form ((pickup 3) A T1) 
; and returns its instantiated start conditions. 
(defun start-instance (iskill)
  (let* ((iargs (cdr iskill))
	 (clauses (cdr (assoc (caar iskill) sltm*)))
	 (clause (car (member (cadar iskill) clauses :key #'sclause-id)))
	 (vargs (clause-args clause))
	 (bindings (mapcar #'cons vargs iargs)))
    (subst-bindings bindings (clause-start clause))))
|#

(defun cstore-and-pop (goal goal-stack)
  (let* ((skills (reverse (mapcar #'sinstance-goal (goal-skills goal))))
	 (subgoals (reverse (goal-subgoals goal)))
	 (achieved (goal-achieved goal))
         (objective (goal-objective goal))
         (bindings (reverse (generate-bindings (cons objective skills) nil)))
         (iargs (mapcar 'car bindings))
         (supergoal (cadr goal-stack))
	 ; Nan's changes below for STARTS and NEW-INSTANCE
	 (starts (sinstance-start (car (last (goal-skills goal)))))
         (new-instance
	      (create-goal-skills objective skills subgoals
				  (append achieved starts))))
    (cond ((not (null supergoal))
           (store-with-supergoal (goal-chaintype supergoal) new-instance
                                 (goal-objective goal) supergoal)
           (popd goal-stack))
	  (t
	   (clear-goal-attributes goal)))))

; CREATE-GOAL-SKILLS inputs a desired concept instance and an ordered list 
; of subgoals that achieves it. The function creates a set of hierarchical 
; skill with the same name as the objective in a right-recursive manner, 
; with the initially satisfied subskills as start conditions on the first,
; these plus the second subgoal as start conditions on the second, and 
; all but the last as start conditions on the last. The function involves
; no analytical collection of conditions. E.g., the objective might be 
; (unstackable C B) and the subgoals might be (((clear 1) B)((hand-empty 2))). 

; (defun create-goal-skills (objective skills subgoals achieved)
;   (let* ((durations (mapcar #'(lambda (s) (get-duration s)) skills)))
;     (create-goal-skill objective subgoals achieved durations)))

; (defun create-goal-skills (objective skills subgoals achieved)
;   (let* ((durations (mapcar #'(lambda (s) (get-duration s)) skills))
; 	 (satisfied (are-already-satisfied objective subgoals)))
;     (create-goal-skill objective (make-monotonic subgoals)
; 		       satisfied durations)))

(defun create-goal-skills (objective skills subgoals achieved)
  (create-goal-skill objective (make-monotonic subgoals) achieved 1))

; (defun create-goal-skills (objective skills subgoals achieved)
;   (let* ((durations (mapcar #'(lambda (s) (get-duration s)) skills))
; 	 (subgoals (expand-recurrers (car objective) skills))
; 	 (subgoals (mapcar #'(lambda (s) (reformat-skill s)) skills)))
;     (create-goal-skill objective (make-monotonic subgoals)
; 		       achieved durations)))

(defun expand-recurrers (obj-name skills)
  (cond ((null skills) nil)
	((eq (caaar skills) obj-name)
	 (append (expand-recurrence (car skills))
		 (expand-recurrers obj-name (cdr skills))))
	(t (cons (car skills)
		 (expand-recurrers obj-name (cdr skills))))))

(defun expand-recurrence (skill)
  (let* ((clauses (cdr (assoc (caar iskill) sltm*)))
         (clause (car (member (cadar iskill) clauses :key #'sclause-id)))
	 (bindings (mapcar #'cons (clause-args clause) (cdr skill))))
(terpri)(princ "Expanding ")(princ skill)
(terpri)(princ "with ")(princ (subst-bindings bindings (clause-ordered skill)))
    (subst-bindings bindings (clause-ordered skill))))

(defun make-monotonic (subgoals)
  (cond ((null subgoals) nil)
	((member (car subgoals) (cdr subgoals) :test 'equal)
	 (make-monotonic (cdr subgoals)))
	(t (cons (car subgoals) (make-monotonic (cdr subgoals))))))

; (defun create-goal-skill (objective subgoals achieved durations)
;   (cond ((null (cdr subgoals))
; 	 (create-one-skill objective (car subgoals)
; 			   (car achieved) (car durations)))
; 	(t (create-goal-skill objective (cdr subgoals) 
; 			      (cdr achieved) (cdr durations))
; 	   (create-two-skill objective (car subgoals) (car achieved)
; 			     (add-durations durations)))))

; (defun create-goal-skill (objective subgoals satisfied durations)
;   (cond ((null (cdr subgoals))
; 	 (create-one-skill objective (car subgoals) satisfied (car durations)))
; 	(t (create-goal-skill objective (cdr subgoals)
; 			      (cons (car subgoals) satisfied)
; 			      (cdr durations))
; 	   (create-two-skill objective (car subgoals) satisfied
; 			     (add-durations durations)))))

(defun create-goal-skill (objective subgoals satisfied durations)
  (let* ((literals (cons objective (append satisfied subgoals)))
	 (bindings (generate-bindings literals nil))
	 (vargs (subst-bindings bindings (cdr objective)))
	 (percepts (get-argument-types (mapcar #'car bindings))))
    (cache-skill (car objective) vargs percepts satisfied
		 nil subgoals bindings 1)))

; CREATE-TWO-SKILL inputs a desired concept instance, a subgoal of that
; objective, a set of subgoals that have already been satisfied, and a
; duration. It stores a right-recursive skill that has the same name and
; arguments as the objective, the subgoal as its first subskill, and the
; objective as its second subskill. 

(defun create-two-skill (objective subgoal satisfied duration)
  (let* ((name (car objective))
	 (literals (cons objective (cons subgoal satisfied)))
	 (bindings (generate-bindings literals nil))
	 (vargs (subst-bindings bindings (cdr objective)))
	 (percepts (remove-duplicates (mapcan #'get-object-types literals)))
	 (skills (list subgoal objective)))
    (cache-skill name vargs percepts satisfied nil skills bindings duration)))

; CREATE-ONE-SKILL inputs a desired concept instance, a subgoal of that
; objective, a set of subgoals that have already been satisfied, and a
; duration. It stores a nonrecursive skill that has the same name and
; arguments as the objective and the subgoal as its only subskill. 

(defun create-one-skill (objective subgoal satisfied duration)
  (let* ((name (car objective))
	 (literals (cons objective (cons subgoal satisfied)))
	 (bindings (generate-bindings literals nil))
	 (vargs (subst-bindings bindings (cdr objective)))
	 (percepts (remove-duplicates (mapcan #'get-object-types literals)))
	 (skills (list subgoal)))
    (cache-skill name vargs percepts satisfied nil skills bindings duration)))

(defun cache-skill (name vargs percepts start end skills bndngs duration)
  (let ((vpercepts (subst-bindings bndngs percepts))
	(vstart (subst-bindings bndngs start))
	(vskills (subst-bindings bndngs skills))
	(existing-clause
	 (not-novel-clause name vargs percepts start end skills)))
    (cond ((null existing-clause)
	   (setq id-count* (1+ id-count*))
	   (push id-count* learned-skills*)
	   (let ((clause
		  (create-skill-clause
		   (list (cons name vargs) ':id id-count* ':percepts vpercepts
			 ':subgoals vskills ':start vstart ':duration duration))))
	     (nconc sltm* (list clause))
	     (cond ((not (null ltrace*))
		    (terpri)(princ "Storing new skill clause: ")
		    (print-clause clause))))
	   (make-sinstance :goal (cons name (unsubst-bindings bndngs vargs))
			  :id id-count* :bindings bndngs :start start))
	  (t (cond ((not (null ltrace*))
		    (terpri)(princ "New clause would be equivalent to ")
		    (princ name)(princ " ")(princ (cadr existing-clause))))
	     (cons existing-clause (unsubst-bindings bndngs vargs))))))

; NOT-NOVEL-CLAUSE inputs the name, arguments, percepts, start conditions, 
; effects, and subskills of a candidate skill, along with a list of known
; skills. If the proposed skill is isomorphic to an existing one, the 
; function returns a list containing its name and id number, but otherwise
; returns NIL. 

(defun not-novel-clause (name args percepts start end skills)
  (not-novel-aux
       (append (list (cons 'head (cons name args))
		     (cons 'skill1 (car skills))
		     (cons 'skill2 (cadr skills)))
	       (mapcar #'(lambda (p) (cons 'percept p)) percepts)
	       (mapcar #'(lambda (s) (cons 'start s)) start))
       sltm*))

(defun not-novel-aux (labeled clauses)
  (cond ((null clauses) nil)
	((not-novel-one labeled (car clauses))
	 (make-sinstance :goal (cdr (assoc 'head labeled))
			 :start (cdr (assoc 'start labeled))))
	(t (not-novel-aux labeled (cdr clauses)))))

(defun not-novel-one (labeled clause)
  (let* ((head (cons 'head (sclause-head clause)))
	 (skill1 (cons 'skill1 (car (sclause-subgoals clause))))
	 (skill2 (cons 'skill2 (cadr (sclause-subgoals clause))))
	 (percepts (mapcar #'(lambda (p) (cons 'percept p))
			   (sclause-percepts clause)))
	 (starts (mapcar #'(lambda (s) (cons 'start s))
			 (sclause-start clause)))
	 (old (append (list head skill1 skill2) percepts starts)))
;   (car (car (match-bconds labeled old nil nil)))))
    (and (not (null (car (car (match-bconds labeled old nil nil T)))))
	 (not (null (car (car (match-bconds old labeled nil nil T))))))))

; UNSUBST-BINDINGS is just like SUBST-BINDINGS except that it replaces 
; variables with constants, rather than the reverse. 

(defun unsubst-bindings (bindings generic)
  (cond ((null bindings) generic)
        (t (unsubst-bindings (cdr bindings)
	            (subst (caar bindings) (cdar bindings) generic)))))

(defun get-object-types (cinst)
  (let* ((cdef (cassoc (car cinst) cltm*))
	 (vargs (cdr (concept-head cdef)))
	 (bindings (mapcar #'cons vargs (cdr cinst))))
    (remove-if #'null (mapcan #'(lambda (binding)
        (remove-if #'null (mapcar #'(lambda (percept)
            (if (eq (car binding) (second percept))
                 (list (first percept) (cdr binding))))
                       (concept-percepts cdef))))
                                bindings))))

; **********************************************************************
; Functions for generating the names of skill and concept variables
; **********************************************************************

; GEN-NAME generates a unique name that starts with TYPE.  

(defun gen-name (type)
  (let ((pair (assoc type names*)))
    (cond ((null pair)
           (push (cons type 1) names*)
           (intern (concatenate 'string (string type)
                                "-" (princ-to-string 1))))
          (t (setf (cdr pair) (1+ (cdr pair)))
             (intern (concatenate 'string (string type) "-" 
                                  (princ-to-string (cdr pair))))))))

; GEN-VAR generates a unique variable name that starts with ?TYPE.  

(defun gen-var (type)
  (let ((pair (assoc type vnames*)))
    (cond ((null pair)
           (push (cons type 1) vnames*)
           (intern (concatenate 'string "?" (string type)
                                (princ-to-string 1))))
          (t (setf (cdr pair) (1+ (cdr pair)))
             (intern (concatenate 'string "?" (string type)  
                                  (princ-to-string (cdr pair))))))))

; GET-ARGUMENT-TYPES inputs a list of object names (O1 O2 ... On) and
; returns a list ((TYPE1 O1) (TYPE2 O2) ... (TYPEn On)) that specifies 
; their respective types. 

(defun get-argument-types (objects)
  (cond ((null objects) nil)
	(t (let ((percept (member (car objects) pstm* :key #'cadr)))
	     (cond ((null percept)
		    (get-argument-types (cdr objects)))
		   (t (cons (list (caar percept) (car objects))
			    (get-argument-types (cdr objects)))))))))
